#include "diskio.h"
#include <stdio.h>
#include <lpc17xx_ssp.h>
#include <lpc17xx_gpio.h>
#include <lpc17xx_pinsel.h>
//SSP pins
#define SDC_SSP_PORT        (0)
#define SDC_MOSI_PIN        (9)
#define SDC_MISO_PIN        (8)
#define SDC_SCLK_PIN        (7)
#define SDC_SSEL_PIN        (6)
#define SDC_CLK_RATE        (400000) //4Khz
//SDC R1 response
#define SDC_RESP_ROK        (0 << 0)
#define SDC_RESP_IDL        (1 << 0)
#define SDC_RESP_RST        (1 << 1)
#define SDC_RESP_CMD        (1 << 2)
#define SDC_RESP_CRC        (1 << 3)
#define SDC_RESP_SEQ        (1 << 4)
#define SDC_RESP_ADR        (1 << 5)
#define SDC_RESP_ARG        (1 << 6)
//SDC commands
#define SDC_GO_IDLE_STATE   (0)
#define SDC_SET_BLOCKLEN    (0x10)
#define SDC_STOP_TRANS      (12)
#define SDC_RD_BLOCKS       (18)
#define SDC_WR_BLOCKS       (25)
#define SDC_RD_DTOKEN       (0xFE)
#define SDC_WR_DTOKEN       (0xFC)
#define SDC_BLOCK_LEN       (512)
#define SDC_NUM_RETRY       (100)     
#define SDC_HOST_FLAG       (0x40)

#define GPIO_OUT            (1)

static uint8_t disk_init=0;

uint8_t ssp_read()
{
    while(SSP_GetStatus(LPC_SSP1, SSP_STAT_BUSY));
    SSP_SendData(LPC_SSP1, 0xFF);
    return SSP_ReceiveData(LPC_SSP1);
}

void ssp_write(uint8_t c)
{
    while(SSP_GetStatus(LPC_SSP1, SSP_STAT_BUSY));
    SSP_SendData(LPC_SSP1, c);
}

void ssp_init(uint32_t clock_rate)
{
    PINSEL_CFG_Type pin_cfg={
        .Pinmode    = PINSEL_PINMODE_PULLUP,
        .OpenDrain  = PINSEL_PINMODE_NORMAL
    };
    
    SSP_CFG_Type  ssp_cfg ={
        .CPHA = SSP_CPHA_FIRST,
        .CPOL = SSP_CPOL_HI,
        .ClockRate = clock_rate,
        .Databit = SSP_DATABIT_8,
        .Mode = SSP_MASTER_MODE,
        .FrameFormat = SSP_FRAME_SPI
    };

    //PINSEL config
    pin_cfg.Funcnum = 2;
    pin_cfg.Portnum = SDC_SSP_PORT;
    //MOSI
    pin_cfg.Pinnum = SDC_MOSI_PIN;
    PINSEL_ConfigPin(&pin_cfg);
    //MISO
    pin_cfg.Pinnum = SDC_MISO_PIN;
    PINSEL_ConfigPin(&pin_cfg);
    //SCLK
    pin_cfg.Pinnum = SDC_SCLK_PIN;
    PINSEL_ConfigPin(&pin_cfg);
    //SSEL GPIO
    pin_cfg.Funcnum = 0;
    pin_cfg.Pinnum = SDC_SSEL_PIN;
    PINSEL_ConfigPin(&pin_cfg);

    GPIO_SetDir(SDC_SSP_PORT, (1<<SDC_SSEL_PIN), GPIO_OUT);
    GPIO_ClearValue(SDC_SSP_PORT, (1<<SDC_SSEL_PIN));

    SSP_Init(LPC_SSP1, &ssp_cfg);
    SSP_Cmd(LPC_SSP1, ENABLE);

    printf("ssp initialized clk %d\n", clock_rate);
}

void sdc_command(uint8_t cmd, uint32_t arg, uint8_t crc) 
{
    ssp_write(SDC_HOST_FLAG | cmd);
    ssp_write(arg >> 24); //arg0
    ssp_write(arg >> 16); //arg1
    ssp_write(arg >> 8);  //arg2
    ssp_write(arg >> 0);  //arg3
    ssp_write(crc);
}

uint8_t sdc_response()
{
    register uint32_t r = 0;
    register uint32_t i = SDC_NUM_RETRY;
    while(((r=ssp_read()) & 0x80) && --i);   
    return r;
}

DSTATUS disk_initialize(BYTE sd)
{
    register uint32_t i,x;

    //initialize SSP
    ssp_init(SDC_CLK_RATE);

    //set cs high and pulse clk 80 times 10 * 8
    GPIO_SetValue(SDC_SSP_PORT, (1<<SDC_SSEL_PIN));    
    for (i=0; i<10; i++) {
        ssp_write(0xFF);
    }

    //switch card to SPI mode
    GPIO_ClearValue(SDC_SSP_PORT, (1<<SDC_SSEL_PIN));    
    sdc_command(SDC_GO_IDLE_STATE, 0, 0x95);

    //read response
    if (sdc_response() != SDC_RESP_IDL) { 
        printf("sdc spi mode failed\n");
        goto error;
    }

    sdc_command(0x08, 0x000001aa, 0x87);
    for (i=0; i<5; i++) {
         sdc_response();
    }

    //TODO handle v1.x cards
    //check cmd is legal and voltage is supported    

    //initialize card
    i = 1000;
    do {
        sdc_command(55, 0, 0x01);
        sdc_response();
        sdc_command(41, (1<<30), 0x01);
        x = sdc_response();
        sleep(1);
    } while (x != SDC_RESP_ROK && --i);

    if (i==0) {
        printf("sdc initialization timeout\n");
        goto error;
    }

    //set bus to highest clock
    ssp_init(12500000); //12Mhz
    
    disk_init = 1;
    printf("sdc initialized\n");
    return 0;
error:
    return STA_NOINIT | STA_NODISK;
}

DSTATUS disk_status(BYTE sd)
{
    return (disk_init == 1) ? STA_OK : (STA_NOINIT | STA_NODISK);
}

//Multiple block read
DRESULT disk_read(BYTE sd, BYTE* buf, DWORD sector_num, BYTE sector_count)
{
    uint16_t i;
    //request block
    sdc_command(SDC_RD_BLOCKS, sector_num * SDC_BLOCK_LEN, 0x01);

    //read response
    if (sdc_response() != SDC_RESP_ROK) {
        printf("multi_block read invalid response\n");
        goto error;
    }
 
    while (sector_count--) {    
        //read data token
        while (ssp_read() != SDC_RD_DTOKEN);

        //read block
        i = SDC_BLOCK_LEN;
        while (i--) {
           *buf++ = ssp_read();
        }
    
        //skip crc
        ssp_read();
        ssp_read();
    }

    //stop transfer
    sdc_command(SDC_STOP_TRANS, 0, 0x01);
    //read response
    while(ssp_read() != SDC_RESP_ROK);
    //busy wait        
    while (ssp_read() != 0xFF);
    return RES_OK;
error:
    return RES_ERROR;
}

DRESULT disk_write(BYTE Drive, const BYTE *buf, DWORD sector_num, BYTE sector_count)
{
    uint16_t i;

    sdc_command(SDC_WR_BLOCKS, sector_num * SDC_BLOCK_LEN, 0x01);

    if (sdc_response() != SDC_RESP_ROK) {
        printf("multi_block write invalid response\n");
        goto error;
    }

    while (sector_count--) {    
        //write data_token
        ssp_write(SDC_WR_DTOKEN);

        //write block
        i = SDC_BLOCK_LEN;
        while (i--) {
            ssp_write(*buf++);
        }
            
        //crc is disabled write any values
        ssp_write(0xFE);
        ssp_write(0xFE);
       
        if (sdc_response() != SDC_RESP_ROK) {
            printf("multi_block write invalid response\n");
            goto error;
        }
        //busy wait        
        while (ssp_read() != 0xFF);
    }

    //stop transfer
    sdc_command(SDC_STOP_TRANS, 0, 0x01); //r1b response
    //read response
    while(ssp_read() != SDC_RESP_ROK);
    //busy wait
    while (ssp_read() != 0xFF);
    return RES_OK;
error:
    return RES_ERROR;
}

DRESULT disk_ioctl(BYTE Drive, BYTE Command, void* Buffer)
{
    return RES_ERROR;
}

DWORD get_fattime (void)
{
    return 0;
}

